#!/usr/bin/env python

##
##
# required for ROS Python
import rospy
from roslib import message
# Standard messages
from std_msgs.msg import Header
from std_msgs.msg import String

# Group6 messages and services


# Image messages (for debugging and configuration)
from sensor_msgs.msg import Image

import sensor_msgs.point_cloud2 as pc2

from sensor_msgs.msg import PointCloud2
from sensor_msgs.msg import PointField

# for converting from ROS-Image to OpenCV-Image and vice versa
from cv_bridge import CvBridge, CvBridgeError

# Import Python's math libraty, advanced number thingie and openCV
import math
import numpy
import cv2

# Publisher and stuff
pub = rospy.Publisher('detected_floor_image', Image, queue_size=10)

# Converter for cv->ros and ros->cv
bridge = CvBridge()

# Message


depthData = 0
imageData = 0

# this one create a plane of the floor,
# a = angle of kinect (degrees), h = height of kinect from the floor
def createFloorPlane(a, h):
	# beta angle used for calculations, in radians
	b = (180-90-(90-a))*math.pi/180
	# three points on the floor at the given angle and height
	#p1 = (-0.2, 0, h / math.sin(b))
	#p2 = (0.2, 0, h / math.sin(b))
	#p3 = (0, h*math.cos(b), h*math.sin(b))
	p1 = (-0.2, -h, 1)
	p2 = (0.2, -h, 1)
	p3 = (0, -h, 0)
	# two vectors the three previous points make
	v1 = (p1[0] - p3[0], p1[1] - p3[1], p1[2] - p3[2])
	v2 = (p2[0] - p3[0], p2[1] - p3[1], p2[2] - p3[2])
	# calculate the cross product to get the normal vector
	n = numpy.cross(v1, v2)
	# calculate the d with the point d3
	d = n[0]*p3[0] + n[1]*p3[1] + n[2]*p3[2]
	# return the plane
	return (n[0], n[1], n[2], -d) # a, b, c, d

# this one returns the distance between a plane and a point
# if ret > 0 then the point is above the plane (front of) and if ret < 0 behind the pland
def getPlanePointDistance(plane, point):
	p = plane
	d = point
	return p[0]*d[0] + p[1]*d[1] + p[2]*d[2] + p[3]
	#return (math.fabs(p[0]*d[0] + p[1]*d[1] + p[2]*d[2] + p[3]) / math.sqrt(math.pow(p[0],2)+math.pow(p[1],2)+math.pow(p[2],2)))

# is floor or not, returns true if yes and false if no, t = tolerance
def isFloor(plane, point, t):
	if (getPlanePointDistance(plane, point) >= t):
		return True
	return False


#calculate a point in the world in respect to kinect
# d depth (mm), y = pixel y, x = pixelx
def calculatePoint(data):
	return (data[0], data[1], data[2])


# the plane
plane = createFloorPlane(45, 0.395)

# This is a simple callback about the subscription to the image topic
def calculate():
		global pub
		global colorData
		global depthData
		global plane
		#colorData = 1 #temp
		if (depthData and colorData):
			rospy.loginfo("calculating")
			i = colorData#.copy()
			d = depthData#.copy()
			
			# Convert from sensor_msgs/Image to OpenCV format
			color = bridge.imgmsg_to_cv2(i, "bgr8")
			# 
			
			# get the dimension
			#h,w,d = res.shape
			h = d.height
			w = d.width
			
			# create mask
			mask = numpy.zeros((h,w), numpy.uint8)

			# the point
			step = 4
			for x in range(0,w, step):
				for y in range(0,h, step):
					#b,g,r = res[y,x]
					point = calculatePoint(read_depth(x, y, depthData))
					if (isFloor(plane, point, 0.02)):
						cv2.rectangle(mask, (x,y), (x+step,y+step), (0,0,0), -1)
					else:
						cv2.rectangle(mask, (x,y), (x+step,y+step), (255,255,255), -1)


			final = cv2.bitwise_and(color, color, mask = mask)			
			
			# Convert img to Ros and Publish
			image_message = bridge.cv2_to_imgmsg(final, "bgr8")
			pub.publish(image_message)

		
#lol
def getColorImage(data):
	global colorData
	colorData = data

# read the depth data
def read_depth(width, height, data):
	#read function
	if (height >= data.height) or (width >= data.width):
		return 0#-1
	data_out = pc2.read_points(data, field_names=None, skip_nans=False, uvs=[[width, height]])
	int_data = next(data_out)
	#rospy.loginfo("int_data"+str(int_data))
	return int_data
		
# get the depth data
def depth_callback(data):
	global depthData
	# pick a height
	#height = int(data.height / 2)
	# pick x coords near front and center
	#middle_x = int(data.width / 2)
	# examine point
	#middle = read_depth(middle_x, height, data)
	# do stuff with middle??!
	
	# old code
	depthData = data
	#rospy.loginfo("width"+str(data.width))
	#rospy.loginfo("height"+str(data.height))
	#rospy.loginfo("")		


# main code
def floor_detection():
	# init
	rospy.init_node('floor_detection', anonymous=True)
	rospy.loginfo("Starting experimental floor detection")

	# Subscribers
	rospy.Subscriber('camera/rgb/image_color', Image, getColorImage)
	rospy.Subscriber('camera/depth_registered/points', PointCloud2, depth_callback)
	
	r = rospy.Rate(10) #10Hz, added    

	# Start the service

	
	#rospy.loginfo("OD: Service enabled")

	while not rospy.is_shutdown():                        
		#arr = "LOL"
		# LOLOLO		
		# log
		calculate()
		# publish
		# pub.publish(arr)
		r.sleep()
	
	# spin() simply keeps python from exiting until this node is stopped
	#rospy.spin()
		
if __name__ == '__main__':
	try:
		floor_detection()
	except rospy.ROSInterruptException: pass



